import image, math, time
import os
import sys
import binascii
import ustruct

from media.sensor import *
from media.display import *
from media.media import *
from time import ticks_ms
from machine import FPIOA
from machine import Pin
from machine import UART

sensor = None

uart = UART(UART.UART2, baudrate=115200, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)

def send_three_uchar(c1, c2, c3):
    global uart
    header = 0xAA
    total_bits = 6
    data_parts = [header, total_bits, c1, c2, c3]
    data = ustruct.pack("<BBBBB", *data_parts)
    checksum = sum(data_parts)
    data += ustruct.pack("<B", checksum)
    if uart is not None:
        uart.write(data)
        hex_data = binascii.hexlify(data).decode('utf-8')
        print(f"发送16进制数据:{hex_data}")
    return data

try:
    print("camera_test")
    fpioa = FPIOA()
    fpioa.help()
    fpioa.set_function(33, FPIOA.GPIO33)
    fpioa.set_function(35, FPIOA.GPIO35)
    fpioa.set_function(11, FPIOA.UART2_TXD)
    fpioa.set_function(12, FPIOA.UART2_RXD)

    pin = Pin(33, Pin.OUT)
    pin.value(0)

    reset_pin = Pin(35, Pin.IN, Pin.PULL_UP)

    sensor = Sensor(width=640, height=640)
    sensor.reset()

    sensor.set_framesize(width=640, height=640)
    sensor.set_pixformat(Sensor.RGB565)

    Display.init(Display.ST7701, width=800, height=480, to_ide=True)

    MediaManager.init()

    sensor.run()
    clock = time.clock()

    pin.value(1)
    flag = 0
    center_x = 0
    center_y = 0
    delta_x, delta_y = 0, 0
    reset_pin.value(1)

    # 新增变量：顶点跟踪功能
    target_index = 0        # 当前目标顶点索引 (0-3)
    target_reached = False  # 是否到达当前目标
    next_target_ready = True # 是否准备好切换到下一个目标
    corner_points = [(0, 0), (0, 0), (0, 0), (0, 0)]  # 存储矩形四个顶点
    detection_counter = 0   # 检测计数器
    frame_counter = 0       # 总帧数

    while True:
        frame_counter += 1
        clock.tick()
        os.exitpoint()
        img = sensor.snapshot(chn=CAM_CHN_ID_0)

        if reset_pin.value() == 0:
            time.sleep_ms(2000)
            for i in range(5):
                img_rect = img.to_grayscale(copy=True)
                img_rect = img_rect.binary([(82, 212)])
                rects = img_rect.find_rects(threshold=10000)

                if rects is not None:
                    for rect in rects:
                        corner = rect.corners()
                        corner_points = corner  # 保存矩形四个顶点
                        detection_counter += 1

                        # 绘制矩形边界
                        img.draw_line(corner[0][0], corner[0][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                        img.draw_line(corner[2][0], corner[2][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                        img.draw_line(corner[2][0], corner[2][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)
                        img.draw_line(corner[0][0], corner[0][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)

                        # 绘制并标记四个顶点
                        for idx, (x, y) in enumerate(corner):
                            color = (255, 0, 0) if idx == target_index else (255, 255, 255)
                            img.draw_circle(x, y, 5, color=color, fill=True)
                            img.draw_string(x+10, y-10, f"{idx}", color=color)

                        # 如果准备好切换目标，更新当前目标点
                        if next_target_ready:
                            target_x, target_y = corner[target_index]
                            next_target_ready = False
                            target_reached = False
                            print(f"新目标点: 顶点 {target_index}, 坐标: ({target_x}, {target_y})")

                if len(rects) == 2:
                    img.compressed_for_ide()
                    Display.show_image(img)
                    print(f"center_point: {[round(center_x), round(center_y)]}")
                    print(f"检测到 {len(rects)} 个矩形，总检测次数: {detection_counter}")
                    flag = 1
                    time.sleep_ms(2000)
                    break

        if flag == 1:
            pin.value(1)

            blobs = img.find_blobs([(), ()], False, x_stride=5, y_stride=5, pixels_threshold=15, margin=True)

            if blobs and len(blobs) > 0:
                blob = blobs[0]  # 处理第一个blob
                x = blob.cx()
                y = blob.cy()

                img.draw_circle(x, y, 5, color=(255, 0, 0), fill=True)
                img.draw_rectangle(blob.x(), blob.y(), blob.w(), blob.h(), color=(0, 255, 0), thickness=2, fill=False)

                # 如果有有效的目标点，计算到目标点的delta
                if not next_target_ready:
                    target_x, target_y = corner_points[target_index]
                    delta_x = target_x - x
                    delta_y = target_y - y

                    # 绘制当前目标点
                    img.draw_circle(target_x, target_y, 8, color=(255, 255, 0), thickness=3)
                    img.draw_string(target_x+10, target_y-10, f"T{target_index}", color=(255, 255, 0))

                    # 检查是否到达目标点 (阈值可调整)
                    if abs(delta_x) < 5 and abs(delta_y) < 5:  # 位置容差
                        if not target_reached:
                            print(f"已到达目标点 {target_index}")
                            target_reached = True

                            # 延迟一段时间后切换到下一个目标
                            time.sleep_ms(1000)

                            # 更新目标索引
                            target_index = (target_index + 1) % 4
                            next_target_ready = True
                else:
                    delta_x, delta_y = 0, 0
            else:
                delta_x, delta_y = 0, 0

        # 发送数据（添加错误处理）
        try:
            send_three_uchar(int(delta_x), int(delta_y), target_index)
        except Exception as e:
            print(f"发送数据时出错: {e}")

        # 在图像上显示信息
        img.draw_string_advanced(50, 50, 80, f"fps: {clock.fps()}", color=(255, 0, 0))
        img.draw_string_advanced(50, 70, 80, f"目标点: {target_index}", color=(255, 0, 0))
        img.draw_string_advanced(50, 90, 80, f"delta: ({int(delta_x)}, {int(delta_y)})", color=(255, 0, 0))
        img.draw_string_advanced(50, 110, 80, f"检测次数: {detection_counter}", color=(255, 0, 0))
        img.draw_string_advanced(50, 130, 80, f"总帧数: {frame_counter}", color=(255, 0, 0))

        img.midpoint_pool(2, 2)
        img.compressed_for_ide()
        Display.show_image(img, x=(800-320)//2, y=(480-320)//2)

except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    if isinstance(sensor, Sensor):
        sensor.stop()
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    MediaManager.deinit()
